#include "iostream"
#include <ros/ros.h>
#include <vector>
#include <objectPoint.h>
#include <featureObjectExtractor.h>
#include <LSfeatureObject.h>
#include <define.h>
#include <CandidateExtractor.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseStamped.h>
#include <robina_human_tracker_lrf/humanTorso.h>
#include <robina_human_tracker_lrf/angle_tracker.h>
#include <boost/thread.hpp>

using namespace std;
using namespace robina_human_tracker_lrf;

/*
*   create By: MJ Namazifar(MJ.Namazifar@qiau.ac.ir)
*                R O B O T
*
*                  X
*                  |
*                  |
*                  |
*   -Y <-----------|---------->Y
*   -90            0         90
*
*
*
*              URG or UBG
*       Laser start form right to left
*
*       127 - 640 ---------> 180 ( -90  , 90  )
*       44  - 725 ---------> 240 ( -120 , 120 )
*                    UTM
*
*/

class TORSO_OBJECT;
class torsoDetector
{
public:
    torsoDetector():nh("~")
    {
        nh.param<std::string>("topic",scan_topic,"/scan_ubg");
        int type;
        nh.param("laser_type",type,0);
        devType = (LRF_DEVICE)type;
        setupLRF(devType);
        initDomainAngleTrack(devType);

        tempCandidate.middlePoint.distance = 120;
        currentCandidate.startPoint.index   = DomainStartIndex;
        currentCandidate.endPoint.index   = DomainEndIndex;
        DEBUG = 0;
    }
public:
    void run();
    void laser_sub(const sensor_msgs::LaserScanConstPtr&);
    void poseHuman_pub(TORSO_OBJECT candideTorso);
    void noHuman_pub();
    void setupLRF(LRF_DEVICE dev);
    void initDomainAngleTrack(LRF_DEVICE dev);

    void filterScan(double StartT, double EndT, double Min, double Max);
    bool track_human(TORSO_OBJECT &pt);
    vector<Candidate > find_candidates(double right, double left, double r);
private:
    double toRadian(double value);
    void   setDomainAnlgeTrack(float alpha);
    bool rescan_follower_srv_cb(angle_tracker::Request& req,
                                angle_tracker::Response& res);
    vector<TORSO_OBJECT> Candidates2Torso_OBJECTS(vector<Candidate> candidates);
private:
    ros::Subscriber laserData_sub;
    ros::Publisher  humanPos2D_pub;
    ros::ServiceServer    rescan_follower_srv;

    boost::mutex mutex;

    int startIndex , endIndex;
    int DomainStartIndex , DomainEndIndex;
    double startAngle;
    double endAngle;
    double reso,step;
    int pointCount;
    int scanCount;
    int SensorFrontStep;
    int DEBUG;
    vector <objectPoint> ObjectPoints;
    vector <double> tempscan;
    featureObjectExtractor featureExtract;
    CandidateExtractor torsoExtract;
    ros::NodeHandle nh;
    std::string scan_topic;
    LRF_DEVICE devType;
    TORSO_OBJECT tempCandidate;
    TORSO_OBJECT currentCandidate;
};

double torsoDetector::toRadian(double value)
{
    return value * M_PI / 180;
}
void torsoDetector::setupLRF(LRF_DEVICE dev)
{
    string str = (dev == UTM)? "UTM":"UBG";
    cout<<" Initialized with "<< str <<" Laser Range Finder Device "<<endl;
    startIndex      =  (dev == UTM)   ? 180 : 128;
    endIndex        =  (dev == UTM) ? 900 : 640;
    pointCount      =  endIndex - startIndex;
    reso            =  360.0 / ( (dev == UTM) ? 1440 : 1024);
    step            =  (dev == UTM) ? 1440 : 1024 / 360.0;
    startAngle      =  -(pointCount ) * reso / 2.0;
    endAngle        =  -startAngle;
    SensorFrontStep =  (dev == UTM) ? 540 : 384;

    double t = startAngle;
    if(DEBUG)
    {
        cout<<" count point : "<<pointCount<<" startAngle : "<<startAngle<<" EndAngle : "<<endAngle;
        cout<<"\n ---------------------------------------\n";
    }
    for (int i = 0; i < pointCount; i++, t+= reso)
    {
        ObjectPoints.push_back( objectPoint( toRadian(t) , i) );
    }
}
void torsoDetector::initDomainAngleTrack(LRF_DEVICE dev)
{
    if(dev == UTM)
    {
        DomainStartIndex = 240;//200
        DomainEndIndex   = 480;//500;
    }
    else
    {
        DomainStartIndex = 180;
        DomainEndIndex   = 360;
    }
}

void torsoDetector::run()
{

    laserData_sub = nh.subscribe<sensor_msgs::LaserScan>("/scan_ubg",1,
                                                         (boost::function < void(const sensor_msgs::LaserScanConstPtr&)>) boost::bind( &torsoDetector::laser_sub, this, _1 ));
    humanPos2D_pub = nh.advertise<robina_human_tracker_lrf::humanTorso>("/humanTorso",1);
    rescan_follower_srv = nh.advertiseService("/rescan_follower",&torsoDetector::rescan_follower_srv_cb,this);

}
void torsoDetector::setDomainAnlgeTrack(float alpha)
{
    float index;
    if (alpha < 0)
    {
        index                             = ( 30 ) + ( 90 + alpha) * step ;
        currentCandidate.startPoint.index =  index;
        currentCandidate.endPoint.index   = (pointCount / 2) + 30;
    }
    else
    {
        index                             = ( pointCount - 30 ) + ( alpha - 90 ) * step;
        currentCandidate.startPoint.index = ( pointCount / 2  ) - 30 ;
        currentCandidate.endPoint.index   =  index;
    }

}
void torsoDetector::laser_sub(const sensor_msgs::LaserScanConstPtr &msg)
{
    for(unsigned i = 0 ; i < msg->ranges.size() ; i++)
    {
        tempscan.push_back(msg->ranges[i] * 100);
    }
    if( track_human( currentCandidate) )
    {

        poseHuman_pub( currentCandidate);
    }
    else
    {
          noHuman_pub();
    }
}

void torsoDetector::filterScan(double StartT, double EndT, double Min, double Max)
{
  for (unsigned i = 0 ; i < tempscan.size() ; i++)
    {

        if (
                i < StartT  ||
             i > EndT ||
             tempscan[i] > Max ||
             tempscan[i] < Min )
        {

            tempscan[i] = 0;
        }
        ObjectPoints[i].setR ( tempscan[i]);
    }
    tempscan.clear();
}

void torsoDetector::poseHuman_pub(TORSO_OBJECT candideTorso)
{
    humanTorso torso;

    torso.human_pos_2d_left.x       =  candideTorso.startPoint.pos2d.x / 100.0;
    torso.human_pos_2d_left.y       =  candideTorso.startPoint.pos2d.y / 100.0;
    //---
    torso.human_pos_2d_centre.x     =  candideTorso.middlePoint.pos2d.x / 100.0;
    torso.human_pos_2d_centre.y     =  candideTorso.middlePoint.pos2d.y /100.0;
    torso.human_pos_2d_centre.theta =  candideTorso.middlePoint.pos2d.theta;
    torso.distance                  =  candideTorso.middlePoint.distance/100;
    //---
    torso.human_pos_2d_right.x      =  candideTorso.endPoint.pos2d.x/100.0;
    torso.human_pos_2d_right.y      =  candideTorso.endPoint.pos2d.y/100.0;
    torso.human_exist               =  1;
    //cout<<"\n --> *** human exist ***\n ";
    if(DEBUG)
    {
        cout<<"\n LX"<<torso.human_pos_2d_left.x ;
        cout<<"\n LY"<<torso.human_pos_2d_left.y;
        cout<<"\n Cx"<<torso.human_pos_2d_centre.x ;
        cout<<"\nCY "<<torso.human_pos_2d_centre.y;
        cout<<"\n Ctet"<<torso.human_pos_2d_centre.theta;
        cout<<"\n dist"<<torso.distance ;
        cout<<"\n Ry"<<torso.human_pos_2d_right.y;
        cout<<"\n Rx"<<torso.human_pos_2d_right.x;
    }
    humanPos2D_pub.publish(torso);
}

void torsoDetector::noHuman_pub()
{
    humanTorso torso;
    torso.human_pos_2d_left.x       =  0;
    torso.human_pos_2d_left.y       =  0;
    //---
    torso.human_pos_2d_centre.x     =  0;
    torso.human_pos_2d_centre.y     =  0;
    torso.human_pos_2d_centre.theta =  0;
    torso.distance                  =  0;
    //---
    torso.human_pos_2d_right.x      =  0;
    torso.human_pos_2d_right.y      =  0;
    torso.human_exist               =  0;
    //cout<<"\n --> No human exist\n ";
    humanPos2D_pub.publish(torso);
}

bool torsoDetector::track_human(TORSO_OBJECT &pt)
{
    int toleranceMin = pt.startPoint.index - TOLERANCEINDEX;
    int toleranceMax = pt.endPoint.index + TOLERANCEINDEX;
    toleranceMin = (toleranceMin < 0)? 0 : toleranceMin;
    toleranceMax = (toleranceMin > 720)? 720 : toleranceMax;

    vector<Candidate> candidates = find_candidates(toleranceMin ,
                                                    toleranceMax ,
                                                    pt.middlePoint.distance + TOLERANCEDISTANCE);
    if (candidates.size() == 0)
    {
        pt.startPoint.index =  DomainStartIndex;
        pt.endPoint.index   =  DomainEndIndex;
        return false;
    }
    vector<TORSO_OBJECT> Torso_Objects = Candidates2Torso_OBJECTS(candidates);
    candidates.clear();
    int target_index = 0;
    float temp = 100000.0;
    for(int  i = 0; i < Torso_Objects.size();i++)
    {
        float tolerance = fabs( pt.middlePoint.distance - Torso_Objects[i].middlePoint.distance );
        if (tolerance < temp)
        {
            temp = tolerance;
            target_index = i;
        }
    }
    if(fabs(tempCandidate.middlePoint.distance - Torso_Objects[target_index].middlePoint.distance) <= DISTANCE_TOLERANCE )
    {
        tempCandidate = (Torso_Objects[target_index]);
        pt = tempCandidate;
        return true;
    }
    return false;
}

vector<Candidate> torsoDetector::find_candidates(double right, double left, double r)
{
    //cout<<"\n right : "<<right<<" left : " << left<< " distance "<< r - TOLERANCEDISTANCE <<endl;
    filterScan( right, left, 30.0 , r);
    featureExtract.setDiagonal(100,0.01);
    vector<LSfeatureObject> objects = featureExtract.doExtract( ObjectPoints , 100 , 10 );//30,8
    //cout<<" object "<<objects.size()<<endl;
    torsoExtract.setObjects(objects);
    torsoExtract.doExtractor( 0 , 0.0 );
    vector<Candidate > candidates = torsoExtract.getCandidates();

    return candidates;
}

bool torsoDetector::rescan_follower_srv_cb(angle_tracker::Request &req, angle_tracker::Response &res)
{
    ROS_INFO("Request for rescan follower recieved");
    setDomainAnlgeTrack(req.alpha);
}

vector<TORSO_OBJECT> torsoDetector::Candidates2Torso_OBJECTS(vector<Candidate> candidates)
{
    vector<TORSO_OBJECT> torso_objects;
    TORSO_OBJECT torso_obj;

    for(int i = 0; i < candidates.size(); i++)
    {
        Candidate candide = (candidates.at(i));

        torso_obj.startPoint.index        = candide.object1_._startIndex;
        torso_obj.endPoint.index          = candide.object1_._endIndex;
        torso_obj.middlePoint.index       = candide.object1_.getMiddleIndex();

        torso_obj.startPoint.pos2d.x      = candide.object1_.getStartPoint().getX();
        torso_obj.startPoint.pos2d.y      = candide.object1_.getStartPoint().getY();
        torso_obj.startPoint.pos2d.theta  = candide.object1_.getStartPoint().getTheta_Deg();
        torso_obj.startPoint.distance     = candide.object1_.getStartPoint().getR();

        torso_obj.middlePoint.pos2d.x     = candide.object1_.getMiddlePoint().getX();
        torso_obj.middlePoint.pos2d.y     = candide.object1_.getMiddlePoint().getY();
        torso_obj.middlePoint.pos2d.theta = candide.object1_.getMiddlePoint().getTheta_Deg();
        torso_obj.middlePoint.distance    = candide.object1_.getMiddlePoint().getR();

        torso_obj.endPoint.pos2d.x        = candide.object1_.getEndPoint().getX();
        torso_obj.endPoint.pos2d.y        = candide.object1_.getEndPoint().getY();
        torso_obj.endPoint.pos2d.theta    = candide.object1_.getEndPoint().getTheta_Deg();
        torso_obj.endPoint.distance       = candide.object1_.getEndPoint().getR();
        if(DEBUG)
        {
            cout<<"\n startPoint.index : "<< torso_obj.startPoint.index;
            cout<<"\n endPoint.index : "  << torso_obj.endPoint.index;
        }
        torso_objects.push_back(torso_obj);
    }
    return torso_objects;
}

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"roboina_humanTracker_LRF");

    torsoDetector torsoDetect;
    cout<<"\n Human Tracker ==> start"<<endl;
    torsoDetect.run();
    ros::Rate loop(1000);
    while(ros::ok())
    {
        ros::spinOnce();
        loop.sleep();
    }
    cout<<"\n Human Tracker ==> finish"<<endl;
    return 0;
}
